#ifndef __MPU6050_H__
#define __MPU6050_H__



/*
mpu6050传感器驱动

以x轴为roll轴(x轴为前进方向）
以y轴为pitch轴
以z轴为yaw轴

可根据实际安装自行转换


!注意IIC速率必需是40kHz

*/



#include "stm_std_lib.h"
#include "config.h"
#include "es_iic.h"
#include "math.h"
#include "Kalman.h"
#include "esapi.h"


#define MPU6050_ADDR 	0xD0	//设备地址(ADO=0)

#define	SMPLRT_DIV		0x19	//陀螺仪采样率，典型值：0x07(125Hz)
#define	MPU_CONFIG		0x1A	//低通滤波频率，典型值：0x06(5Hz)
#define	GYRO_CONFIG		0x1B	//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define	ACCEL_CONFIG	0x1C	//加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)

#define	PWR_MGMT_1		0x6B	//电源管理，典型值：0x00(正常启用)
#define	WHO_AM_I		0x75	//IIC地址寄存器(默认数值0x68，只读)


#define	ACCEL_XOUT_H	0x3B
#define	ACCEL_XOUT_L	0x3C
#define	ACCEL_YOUT_H	0x3D
#define	ACCEL_YOUT_L	0x3E
#define	ACCEL_ZOUT_H	0x3F
#define	ACCEL_ZOUT_L	0x40

#define	TEMP_OUT_H		0x41
#define	TEMP_OUT_L		0x42

#define	GYRO_XOUT_H		0x43
#define	GYRO_XOUT_L		0x44	
#define	GYRO_YOUT_H		0x45
#define	GYRO_YOUT_L		0x46
#define	GYRO_ZOUT_H		0x47
#define	GYRO_ZOUT_L		0x48



class mpu6050 {
public:
	mpu6050();

	//mpu初始化
	void init();

	//更新传感器数据，每次循环调用
	void update();



	//获取传感器值
	float getAccX();
	float getAccY();
	float getAccZ();

	float getGyrX();
	float getGyrY();
	float getGyrZ();


	//计算即时roll角
	float getRoll();

	//计算即时pitch角
	float getPitch();



	//轴初始化(需要静止执行),cnt为累加次数
	void axisInit(s16 cnt);



private:
	u8 data_buf;  //读数据缓存

	//mpu缓存
	u8 mpuData[12];
	


	CI2C m_iic;

	//iic操作
	void write(u8 addr, u8 data);
	u8 read(u8 addr);




private:		//内部运算使用
	s16 acc_cnt, gyr_cnt;        //累加的数量
	float acc_fixX, acc_fixY, acc_fixZ;     //加速度，三轴修正值
	float gyr_fixX, gyr_fixY, gyr_fixZ;     //角速度，三轴修正值

	float accx, accy, accz;	//三轴加速度(g的倍数)
	float gyrx, gyry, gyrz;	//三轴角速度(deg/s)

	//转换16位
	u16 calData(u8 dh, u8 dl);

	//计算加速度
	float acc_Cal(u8 h, u8 l);

	//计算角速度
	float gyr_Cal(u8 h, u8 l);


	//弧度转角度
	float rad2deg(float rad);


	//添加累加值
	void acc_add(float x, float y, float z);

	//添加累加值
	void gyr_add(float x, float y, float z);

	//计算平均值
	void acc_Average(void);

	//计算平均值
	void gyr_Average(void);



	//计算加速度向量的模
	float getAcceleration();



	////////// 卡尔曼滤波 /////////////
public:
	//获取经过卡尔曼滤波的roll(由out_roll输出)。及roll角速度(由out_rollRate输出)，dt单位为秒
	void getKalmanRoll(float dt_second, float & out_roll, float& out_rollRate);

	//获取经过卡尔曼滤波的pitch(由out_pitch输出)。及pitch角速度(由out_pitchRate输出)，dt单位为秒
	void getKalmanPitch(float dt_second, float & out_pitch, float& out_pitchRate);
	
	//获取经过卡尔曼滤波的Yaw(由out_yaw输出)。及yaw角速度(由out_yawRate输出)，dt单位为秒
	void getKalmanYaw(float dt_second, float & out_yaw, float& out_yawRate);


public:
	//卡尔曼对象
	Kalman m_kalmanRoll;
	Kalman m_kalmanPitch;

private:
	//上次的roll pitch yaw
	float m_fLastRoll;
	float m_fLastPitch;




//////////////// roll pitch的高级fix //////////////////////////////////

public:
	//roll pitch修正，应在axisInit之后执行
	void roll_pitch_fix();

	//更新roll pitch(带修正)
	void updateRollPitch(float& roll,float& pitch);

private:
	float rollFix;
	float pitchFix;

	void upRollPitch(float& roll,float& pitch);

	vu64 lastUpdateT;



};




#endif
